/*
 * CPFSensorModel.cpp
 *
 *  Created on: Nov 27, 2010
 *      Author: bipins
 */

#include "CPFSensorModel.h"

CPFSensorModel::CPFSensorModel(ros::NodeHandle n) :
	laser(n) {
	cost_marker_pub = n.advertise<visualization_msgs::Marker> ("visualization_marker", 10);
}

CPFSensorModel::~CPFSensorModel() {
}

void CPFSensorModel::setObstacleCostMap(const MATRIX &m) {
	obstacleCostMap = m;
}

void CPFSensorModel::estimateWeights(const MyPose &robotPose, const PoseArray &personPoses,
		const PoseArray &personPosesMeans, const vector<DD> &personPosesStds, const Robot &robot,
		double &w) {

//	cout << "\t" << personPosesStds[0].first << " " << personPosesStds[0].second << endl;

	// first, get the most-recent laser-scan
	SensorMessages sr = laser.scan();
	float maxRange = sr->range_max;
	float minRange = sr->range_min;
	float angleMin = sr->angle_min, angleMax = sr->angle_max, angleIncrement = sr->angle_increment;

	// fetch robot error-characteristics
	double gaussianVariance = atof(robot.getConfiguration("robot.sensor.phit.variance").c_str());
	int laserAngleStep = atoi(robot.getConfiguration("robot.sensor.skipAngle").c_str());
	double prand = atof(robot.getConfiguration("robot.sensor.prand").c_str());
	double zhit = atof(robot.getConfiguration("robot.sensor.zhit").c_str());
	double zrand = atof(robot.getConfiguration("robot.sensor.zrand").c_str());

	double epsilon = 0.01;
//		cout <<  "Variance " << gaussianVariance << ", laserAngleStep " << laserAngleStep << endl;


//	ROS_INFO("Getting map co-ords");
//	cout << robotPose. << endl;
//	MyPose odomPose = robotPose;
//	ROS_INFO("Done Getting map co-ords");

	// get pose in map-coords

	double mapX, mapY, mapTheta;
	robotPose.getPoseInMap(mapX, mapY, mapTheta);
	PII baseInMap = index(mapX, mapY, CELL_RESOLUTION);
//		cout <<  "Pose in map: " << mapX << ", " << mapY << ", " << mapTheta << endl;

	double poseProbability = 1.0;
	// foreach laser-ray
	for (float angle = angleMin; angle <= angleMax; angle += angleIncrement * laserAngleStep) {

		int rangeIndex = (angle - angleMin) / angleIncrement;
		float distance = sr->ranges[rangeIndex];

		// check for spurious laser-readings
		if (distance < minRange || distance > maxRange) {
			continue;
		}

		// ignore all laser-readings which are at maxRange
		if (fabs(distance - maxRange) < epsilon) {
			continue;
		}

		// determine end of laser-ray
		// end-of-laser-ray = current-robot-point + laser-ray

		double laserX = mapX + distance * cos(mapTheta + angle);
		double laserY = mapY + distance * sin(mapTheta + angle);

		double closestPersonDistance = distanceToClosestPerson(laserX, laserY, personPoses,
				personPosesMeans, personPosesStds);

//		if (closestPersonDistance < 1)	{
//			ROS_INFO("Lx: %0.2f Ly: %0.2f Angle: %0.1f Distance: %0.2f %0.2f %0.2f", laserX, laserY, angle*180/PI+90, closestPersonDistance);
//		}

		PII laserInMap = index(laserX, laserY, CELL_RESOLUTION);
		double indexLaserX = laserInMap.first, indexLaserY = laserInMap.second;

		// Laser has shot out of the map if the following conditions are true
		if (indexLaserX >= obstacleCostMap.size() || indexLaserY >= obstacleCostMap[0].size())
			continue;

		// find closest obstacle to that point in the map
		// Obstacle can either be a wall or a person
		// We find the min of these two distances, with some
		// constraints.
		double closestObstacleDistance = obstacleCostMap[indexLaserX][indexLaserY];

//				std::cout << "cod: " << closestObstacleDistance << "\ncop: " << closestPersonDistance << std::endl;

		closestObstacleDistance = min(closestObstacleDistance, closestPersonDistance);

		// determine laser-match to that point
		double probability = 0.0;
		probability += zhit * gaussianProbability(distance, gaussianVariance, distance
				- closestObstacleDistance);
		probability += zrand * prand;

		// assumes independence of laser-rays
		// and multiplies probabilities.
		poseProbability *= probability;

	} // end laser-angle


	w = poseProbability;

}

/**
 * Implements a nearest-neighbor search for a person,
 * modified such that any laser point outside 3 times the
 * standard deviation of the expected position of a person
 * is considered to be an outlier, and is not
 * assigned to that person
 */
double CPFSensorModel::distanceToClosestPerson(double laserX, double laserY,
		const PoseArray &personPoses, const PoseArray &personPosesMeans,
		const vector<DD> &personPosesStds) {


	double closestDistance = INF;
	int belongsToPerson = -1;

//	if (personPoses.size()){
//		cout << "Size of personPosesMeans " << personPosesMeans.size() << endl;
//		cout << "Size of personPoses " << personPoses.size() << endl;
//	}
	// find the cluster that this laser-point belongs to
	for (int i = 0; i < personPoses.size(); ++i) {

		MyPose mean = personPosesMeans[i];
		DD std = personPosesStds[i];
//		cout << "STD: " << std.first << ", " << std.second << endl;
		double meanX, meanY, meanTheta;
		mean.getPoseInMap(meanX, meanY, meanTheta);


		// check the distance to this person-filter cluster
//		double distance = modifiedEuclideanDistance(meanX, meanY, 30*std.first, 30*std.second, laserX, laserY);
//		double distance = modifiedEuclideanDistance(meanX, meanY, 0.6, 0.6, laserX, laserY);
		double distance = modifiedEuclideanDistance(meanX, meanY, 3*std.first, 3*std.second, laserX, laserY);
		// hard assign
		if (distance < closestDistance) {
			closestDistance = distance;
			belongsToPerson = i;
		}

	}

	if (belongsToPerson != -1) {
		// find closest cluster. Now find the actual distance.
		MyPose person = personPoses[belongsToPerson];
		double personX, personY, personTheta;
		person.getPoseInMap(personX, personY, personTheta);
//		cout << "Found a laser reading that belongs to person " << belongsToPerson << endl;
		return modifiedEuclideanDistance(personX, personY, INF, INF, laserX, laserY);
	} else {
		// point is an outlier to everyone
		return INF;
	}

}

void CPFSensorModel::numberOfUnknownObstacles(const MyPose &robotPose, const PoseArray &personPoses,
					const PoseArray &personPosesMeans, const vector<DD> &personPosesStds,
					const Robot &robot, int &M, STLPose &newPersonPose, const MATRIX &map)	{


	// first, get the most-recent laser-scan
	SensorMessages sr = laser.scan();
	float maxRange = sr->range_max;
	float minRange = sr->range_min;
	float angleMin = sr->angle_min, angleMax = sr->angle_max, angleIncrement = sr->angle_increment;

	// fetch robot error-characteristics
	double gaussianVariance	= atof(robot.getConfiguration("robot.sensor.phit.variance").c_str());
	int laserAngleStep 		= atoi(robot.getConfiguration("robot.sensor.skipAngle").c_str());
	double unknownReadingThreshold 	= atof(robot.getConfiguration("robot.sensor.unknownReading.threshold").c_str());
	int nUnknownReadingsIsAPerson 	= atoi(robot.getConfiguration("robot.sensor.unknownReading.nReadingsIsAPerson").c_str());

	double epsilon = 0.01;
//	cout <<  "Variance " << gaussianVariance << ", laserAngleStep " << laserAngleStep << endl;



	MyPose odomPose = robotPose;

	// get pose in map-coords
	double mapX, mapY, mapTheta;
	odomPose.getPoseInMap(mapX, mapY, mapTheta);
	PII baseInMap = index(mapX, mapY, CELL_RESOLUTION);
//	cout <<  "Pose in map: " << mapX << ", " << mapY << ", " << mapTheta << endl;

	int unknownReadings = 0;

	// foreach laser-ray
	for (float angle = angleMin; angle <= angleMax; angle += angleIncrement*laserAngleStep)	{

		int rangeIndex = (angle - angleMin) / angleIncrement;
		float distance = sr->ranges[rangeIndex];

		// check for spurious laser-readings
		if (distance < minRange || distance > maxRange)	{
			continue;
		}

		// ignore all laser-readings which are at maxRange
		if (fabs(distance - maxRange) < epsilon){
			continue;
		}

		// determine end of laser-ray
		// end-of-laser-ray = current-robot-point + laser-ray


		double laserX = mapX + distance*cos(mapTheta + angle);
		double laserY = mapY + distance*sin(mapTheta + angle);
		double closestPersonDistance = distanceToClosestPerson(laserX, laserY, personPoses, personPosesMeans, personPosesStds);

		PII laserInMap = index(laserX, laserY, CELL_RESOLUTION);
		double indexLaserX = laserInMap.first, indexLaserY = laserInMap.second;

		// Laser has shot out of the map if the following conditions are true
		if(indexLaserX >= obstacleCostMap.size() || indexLaserY >= obstacleCostMap[0].size())
			continue;


		// find closest obstacle to that point in the map
		// Obstacle can either be a wall or a person
		// We find the min of these two distances, with some
		// constraints.
		double closestObstacleDistance = obstacleCostMap[indexLaserX][indexLaserY];


//		std::cout << "cod: " << closestObstacleDistance << "\ncop: " << closestPersonDistance << std::endl;
//		if (closestPersonDistance < 10)	{
//			cout << "\n\n\n\n\n";
//			cout << "=============================\nfound someone closer\n=====================" << endl;
//			cout << "\n\n\n\n\n";
//		}

		closestObstacleDistance = min(closestObstacleDistance, closestPersonDistance);

		// check if closestObstacleDistance is explained
		// i.e it's either due to a wall we know, or a person we expect
		if (closestObstacleDistance > unknownReadingThreshold && map[indexLaserX][indexLaserY] == SPACE)	{
			ROS_INFO("Lx: %0.2f Ly: %0.2f Angle: %0.1f Distance: %0.2f %0.2f %0.2f", laserX, laserY, angle*180/PI+90, closestObstacleDistance, obstacleCostMap[indexLaserX][indexLaserY], closestPersonDistance);
			++unknownReadings;
			newPersonPose.first.first = laserX;
			newPersonPose.first.second = laserY;
			newPersonPose.second = 0;
		}


	} // end laser-angle


	// Increment the number of people by 1 if enough sensor-readings
	// go unexplained. This ensures that only one person is added at
	// a time. Ideally, we would like to ensure that these readings
	// are clustered together, i.e. come from a single source. That's
	// going to be a TODO
	if (unknownReadings > nUnknownReadingsIsAPerson)	{
		cout << "Found nUnknowns " << unknownReadings << endl;
		M = personPoses.size() + 1;
	}
	else	{
		cout << "Found nUnknowns " << unknownReadings << endl;
		M = personPoses.size();
	}

}


/*
 * Returns the euclidean distance between (x1, y1) and (x2, y2)
 * Modified such that if the point (x2, y2) is outside a certain range (x1+stdx, y1+stdy)
 * this fnc. returns INF
 */
double CPFSensorModel::modifiedEuclideanDistance(double x1, double y1, double stdx, double stdy,
		double x2, double y2) {
	//	cout << "\t\t\tmean " << x1 <<  " " << y1 << endl;
	//	cout << "\t\t\tlaser " << x2 <<  " " << y2 << endl;
	//	cout << "\t\t\tstd " << stdx <<  " " << stdy << endl;
	if (fabs(x1 - x2) > stdx || fabs(y1 - y2) > stdy)
		return INF;
	return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

double CPFSensorModel::gaussianProbability(double mean, double variance, double x) {

	return (1 / sqrt(2 * PI * variance)) * exp(-(x - mean) * (x - mean) / (2 * variance));

}
